/*
 * @文件描述: 内插算法集合
 * @版本: 
 * @作者: 周晨阳
 * @Date: 2021-02-15 20:59:52
 */
#include "interpolation.h"
#ifndef WIN_SIM
#include "arm_math.h"

#endif // !WIN_SIM

#define PI (3.1415926f)
/**
 * @brief : sinc函数
 * @param {*}
 * @return {*}
 */
__inline static float sinc(float x)
{
    if (x == 0)
    {
        return 1;
    }
    else
    {
        return arm_sin_f32(PI * x) / (PI * x);
    }
}
/**
 * @brief : 线性插值算法
 * @param {uint16_t} *ori 源数据
 * @param {uint16_t} oriCnt 源数据个数
 * @param {uint16_t} *processed 插值后的数据
 * @param {uint16_t} processedCnt 插值后的数据个数
 * @return {*}
 */
void Linear_Interpolation(uint16_t *ori, uint16_t oriCnt, uint16_t *processed, uint16_t processedCnt)
{

    // int i;
    // int k=processedCnt/oriCnt;
    // for(i=0;i<processedCnt;i++){
    //     processed[i]=ori[i/k];
    // }
    // int i;
    // int x;
    // int k = processedCnt / oriCnt;
    // for (i = 0; i < processedCnt; i++)
    // {
    //     x = (int)(4096.0f / (float)(k * oriCnt));
    //     processed[i] = arm_linear_interp_q31((q31_t *)ori, x, oriCnt);
    //     printf("%d\n",processed[i]);
    // }
    float increment = (float)(oriCnt - 1) / (float)(processedCnt - 1);
    float x = 0;
    int i;
    int x0, x1, y0, y1;
    float tmp = 0;
    for (i = 0; i < processedCnt; i++)
    {
        x0 = (int)((i + 1) * increment - increment);
        x1 = (int)((i + 1) * increment + increment);
        y0 = ori[x0];
        y1 = ori[x1];

        processed[i] = (int)(y0 + (x - x0) * ((y1 - y0) / (x1 - x0)));
        x += increment;
        //printf("%d\n", processed[i]);
    }
}
/**
 * @brief : sinc内插算法
 * @param {uint16_t} *ori 源数据
 * @param {uint16_t} oriCnt 源数据个数
 * @param {uint16_t} *processed 内插后的数据
 * @param {uint16_t} processedCnt 需要内插成的个数
 * @return {*}
 * 后面的数据大小不可以比前面的小
 */
__inline void Sinc_Interpolation(uint16_t *ori, uint16_t oriCnt, uint16_t *processed, uint16_t processedCnt)
{
    int i, j;
    float tmp = 0;
    float t = 0;
    float inside = 0;
    float inside2 = 0;
    float total = 0;
    //t的增量，单位ns
    float t_increment = (float)(oriCnt - 1) / (float)(processedCnt - 1);

    for (j = 0; j < processedCnt; j++)
    {
        for (i = 0; i < oriCnt; i++)
        {
            inside = t - (i);
            inside2 = sinc(inside);
            total = ori[i] * inside2;
            tmp += total;
        }
        t += t_increment;
        //printf("%f\n", tmp);
        processed[j] = tmp;
        tmp = 0;
    }
}